/*
 * Copyright (c) 2012-2014 Wind River Systems, Inc.
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr/kernel.h>

#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/gatt.h>

#include <zephyr/bluetooth/gap.h>

#include <power/reboot.h>

// define connection object
struct bt_conn *my_conn = NULL;
static struct bt_gatt_exchange_params exchange_params;

// create advertising parameters
static struct bt_le_adv_param *adv_param = BT_LE_ADV_PARAM(
	(BT_LE_ADV_OPT_CONNECTABLE |
	 BT_LE_ADV_OPT_USE_IDENTITY), /* Connectable advertising and use identity address */
	800, /* Min Advertising Interval 500ms (800*0.625ms) */
	801, /* Max Advertising Interval 500.625ms (801*0.625ms) */
	NULL); /* Set to NULL for undirected advertising */


#define BT_LE_ADV_CONN_NO_ACCEPT_LIST                                                              \
	BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONNECTABLE | BT_LE_ADV_OPT_ONE_TIME,                        \
			BT_GAP_ADV_FAST_INT_MIN_2, BT_GAP_ADV_FAST_INT_MAX_2, NULL)

#define BT_LE_ADV_CONN_ACCEPT_LIST                                                                 \
	BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONNECTABLE | BT_LE_ADV_OPT_FILTER_CONN |                    \
				BT_LE_ADV_OPT_ONE_TIME,                                            \
			BT_GAP_ADV_FAST_INT_MIN_2, BT_GAP_ADV_FAST_INT_MAX_2, NULL)



// define advertising data
static const struct bt_data ad[] = {
	BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
	BT_DATA(BT_DATA_NAME_COMPLETE, CONFIG_BT_DEVICE_NAME, sizeof(CONFIG_BT_DEVICE_NAME) - 1),
};

// define scan response data
static const struct bt_data sd[] = {
};

static void setup_accept_list_cb(const struct bt_bond_info *info, void *user_data)
{
	int *bond_cnt = user_data;

	if ((*bond_cnt) < 0) {
		return;
	}

	int err = bt_le_filter_accept_list_add(&info->addr);
	printk("Added following peer to accept list: %x %x\n", info->addr.a.val[0],
		info->addr.a.val[1]);
	if (err) {
		printk("Cannot add peer to filter accept list (err: %d)\n", err);
		(*bond_cnt) = -EIO;
	} else {
		(*bond_cnt)++;
	}
}


static int setup_accept_list(uint8_t local_id)
{
	int err = bt_le_filter_accept_list_clear();

	if (err) {
		printk("Cannot clear accept list (err: %d)\n", err);
		return err;
	}

	int bond_cnt = 0;

	bt_foreach_bond(local_id, setup_accept_list_cb, &bond_cnt);

	return bond_cnt;
}

void advertise_with_acceptlist(struct k_work *work)
{
	int err = 0;
	int allowed_cnt = setup_accept_list(BT_ID_DEFAULT);
	if (allowed_cnt < 0) {
		printk("Acceptlist setup failed (err:%d)\n", allowed_cnt);
	} else {
		if (allowed_cnt == 0) {
			printk("Advertising with no Accept list \n");
			err = bt_le_adv_start(BT_LE_ADV_CONN_NO_ACCEPT_LIST, ad, ARRAY_SIZE(ad), sd,
					      ARRAY_SIZE(sd));
		} else {
			printk("Acceptlist setup number  = %d \n", allowed_cnt);
			err = bt_le_adv_start(BT_LE_ADV_CONN_ACCEPT_LIST, ad, ARRAY_SIZE(ad), sd,
					      ARRAY_SIZE(sd));
		}
		if (err) {
			printk("Advertising failed to start (err %d)\n", err);
			return;
		}
		printk("Advertising successfully started\n");
	}
}

K_WORK_DEFINE(advertise_acceptlist_work, advertise_with_acceptlist);

static void update_phy(struct bt_conn *conn)
{
	int err;

	// update PHY
	err = bt_conn_le_phy_update(conn, BT_CONN_LE_PHY_PARAM_2M);
	if (err) {
		printk("PHY update failed (err %d)\n", err);
	} else {
		printk("PHY update pending\n");
	}
}

static void update_data_len(struct bt_conn *conn)
{
	int err;

	struct bt_conn_le_data_len_param param =
	{
		.tx_max_len = BT_GAP_DATA_LEN_MAX,
		.tx_max_time = BT_GAP_DATA_TIME_MAX,
	};
	
	// update data length
	err = bt_conn_le_data_len_update(conn, &param);
	if (err) {
		printk("Data length update failed (err %d)\n", err);
	} else {
		printk("Data length update pending\n");
	}
}

static void exchange_func(struct bt_conn *conn, uint8_t att_err,
			  struct bt_gatt_exchange_params *params)
{
	printk("MTU exchange %s\r\n", att_err == 0 ? "successful" : "failed");
	if (!att_err) {
		uint16_t payload_mtu =
			bt_gatt_get_mtu(conn) - 3; // 3 bytes used for Attribute headers.
		printk("New MTU: %d bytes\n", payload_mtu);
	}
}

static void update_mtu(struct bt_conn *conn)
{
	int err;

	exchange_params.func = exchange_func;

	// update MTU
	err = bt_gatt_exchange_mtu(conn, &exchange_params);
	if (err) {
		printk("MTU exchange failed (err %d)\n", err);
	} else {
		printk("MTU exchange pending\n");
	}
}

void on_connected(struct bt_conn *conn, uint8_t err)
{
	printk("Connected\n");
	my_conn = bt_conn_ref(conn);

	struct bt_conn_info info;
	bt_conn_get_info(conn, &info);

	double connection_interval = info.le.interval * 1.25; // in ms
	uint16_t supervision_timeout = info.le.timeout * 10; // in ms
	printk("Connection parameters: interval %.2f ms, latency %d intervals, timeout %d ms\r\n",
		connection_interval, info.le.latency, supervision_timeout);
	
	update_phy(conn);
	update_data_len(conn);
	update_mtu(conn);

}

void on_disconnected(struct bt_conn *conn, uint8_t reason)
{
	printk("Disconnected (reason %u)\n", reason);
}

void on_le_param_updated(struct bt_conn *conn, uint16_t interval, uint16_t latency,
			 uint16_t timeout)
{
	double connection_interval = interval * 1.25; // in ms
	uint16_t supervision_timeout = timeout * 10; // in ms
	printk("Connection parameters updated: interval %.2f ms, latency %d intervals, timeout %d ms\n",
		connection_interval, latency, supervision_timeout);
}

void on_le_phy_updated(struct bt_conn *conn, struct bt_conn_le_phy_info *param)
{
	// PHY Updated
	if (param->tx_phy == BT_CONN_LE_TX_POWER_PHY_1M) {
		printk("PHY updated. New PHY: 1M\r\n");
	} else if (param->tx_phy == BT_CONN_LE_TX_POWER_PHY_2M) {
		printk("PHY updated. New PHY: 2M\r\n");
	} else if (param->tx_phy == BT_CONN_LE_TX_POWER_PHY_CODED_S8) {
		printk("PHY updated. New PHY: Long Range\r\n");
	}
}

void on_le_data_len_updated(struct bt_conn *conn, struct bt_conn_le_data_len_info *info)
{
	uint16_t tx_len = info->tx_max_len;
	uint16_t tx_time = info->tx_max_time;
	uint16_t rx_len = info->rx_max_len;
	uint16_t rx_time = info->rx_max_time;
	printk("Data length updated. Length %d/%d bytes, time %d/%d us\r\n", tx_len, rx_len, tx_time,
		rx_time);
}

// define connection callbacks
struct bt_conn_cb connection_callbacks = {
	.connected = on_connected,	
	.disconnected = on_disconnected,
	.le_param_updated = on_le_param_updated,
	.le_phy_updated = on_le_phy_updated,
	.le_data_len_updated = on_le_data_len_updated,
};


#define PRIMARY_SERVICE_UUID BT_UUID_DECLARE_16(0x4321) 
#define INDICATE_CHAR_UUID BT_UUID_DECLARE_16(0x4322)
#define WRITE_CHAR_UUID BT_UUID_DECLARE_16(0x4323)
#define NOTIFY_CHAR_UUID BT_UUID_DECLARE_16(0x4324)

bool indicate_flag = false;
bool indicate_enabled = false;
static struct bt_gatt_indicate_params indicate_param;

bool notify_enabled = false;
bool notify_flag = false;

static ssize_t indicate_char_callback(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf,
			   uint16_t len, uint16_t offset)
{
	const char *value = attr->user_data;

	printk("Attribute read, handle: %u, conn: %p\n", attr->handle, (void *)conn);

	return bt_gatt_attr_read(conn, attr, buf, len, offset, value, sizeof(*value));
}

static void indicate_char_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value)
{
	indicate_enabled = (value == BT_GATT_CCC_INDICATE);
}

static void indicate_cb(struct bt_conn *conn, struct bt_gatt_indicate_params *params, uint8_t err)
{
	printk("Indication %s\n", err != 0U ? "fail" : "success");
}

void unpair_device(void)
{

	int err = bt_unpair(BT_ID_DEFAULT, BT_ADDR_LE_ANY);
	if (err) {
		printk("Cannot delete bond (err: %d)\n", err);
	} else {
		printk("Bond deleted succesfully\n");
	}
	k_sleep(K_MSEC(1000));

	int err_code = bt_le_adv_stop();
	if (err_code) {
		printk("Cannot stop advertising err= %d \n", err_code);
		return;
	}
	err_code = bt_le_filter_accept_list_clear();
	k_sleep(K_MSEC(1000));
	if (err_code) {
		printk("Cannot clear accept list (err: %d)\n", err_code);
	} else {
		printk("Accept list cleared succesfully\n");
	}
	k_sleep(K_MSEC(1000));
	err_code = bt_le_adv_start(BT_LE_ADV_CONN_NO_ACCEPT_LIST, ad,
					ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd));

	if (err_code) {
		printk("Cannot start open advertising (err: %d)\n", err_code);
	} else {
		printk("Advertising in pairing mode started\n");
	}
	// sys_reboot(SYS_REBOOT_COLD);
}

bool unpair_flag = false;

static ssize_t write_char_callback(struct bt_conn *conn, const struct bt_gatt_attr *attr, const void *buf,
			 uint16_t len, uint16_t offset, uint8_t flags)
{
	printk("Attribute write, handle: %u, conn: %p\n", attr->handle, (void *)conn);

	if (offset != 0) {
		printk("Write led: Incorrect data offset\n");
		return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET);
	}

	char *value = (uint8_t *)buf;
	printk("Write data: %s\n", value);

	if (*value == 0xFF)
	{
		printk("Unpairing device\n");
		unpair_flag = true;
	}

	return len;

}

static void notify_char_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value)
{
	notify_enabled = (value == BT_GATT_CCC_NOTIFY);
}



BT_GATT_SERVICE_DEFINE(
	custom_service,
	BT_GATT_PRIMARY_SERVICE(PRIMARY_SERVICE_UUID),
	BT_GATT_CHARACTERISTIC(INDICATE_CHAR_UUID, 
				BT_GATT_CHRC_READ | BT_GATT_CHRC_INDICATE,
			    BT_GATT_PERM_READ, 
				indicate_char_callback, 
				NULL, 
				&indicate_flag),
	BT_GATT_CCC(indicate_char_ccc_cfg_changed, 
				BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
	BT_GATT_CHARACTERISTIC(WRITE_CHAR_UUID, 
				BT_GATT_CHRC_WRITE, 
				BT_GATT_PERM_WRITE_AUTHEN, 
				NULL,
			    write_char_callback, 
				NULL),
	BT_GATT_CHARACTERISTIC(NOTIFY_CHAR_UUID, 
				BT_GATT_CHRC_NOTIFY, 
				BT_GATT_PERM_NONE, 
				NULL,
				NULL, 
				NULL),
	BT_GATT_CCC(notify_char_ccc_cfg_changed, 
				BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),

);


int indicate_send(bool value)
{
	if(!indicate_enabled)
	{
		return EACCES;
	}

	indicate_param.attr = &custom_service.attrs[2];
	indicate_param.func = indicate_cb;
	indicate_param.data = &value;
	indicate_param.len = sizeof(value);
	indicate_param.destroy = NULL;

	return bt_gatt_indicate(NULL, &indicate_param);
}

int notify_send(bool value)
{
	if(!notify_enabled)
	{
		return EACCES;
	}

	return bt_gatt_notify(NULL, &custom_service.attrs[6], &value, sizeof(value));
}

static void auth_passkey_display(struct bt_conn *conn, unsigned int passkey)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	printk("Passkey for %s: %06u\n", addr, passkey);
}

static void auth_cancel(struct bt_conn *conn)
{
	char addr[BT_ADDR_LE_STR_LEN];

	bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

	printk("Pairing cancelled: %s\n", addr);
}

static struct bt_conn_auth_cb conn_auth_callbacks = {
	.passkey_display = auth_passkey_display,
	.cancel = auth_cancel,
};


int main(void)
{
	int err;
	
	bt_addr_le_t addr;

	// create device address
	bt_addr_le_from_str("FF:EE:DD:CC:BB:AA", "random", &addr);
	bt_id_create(&addr, NULL);

	// register authentication callbacks
	bt_conn_auth_cb_register(&conn_auth_callbacks);

	// register connection callbacks
	bt_conn_cb_register(&connection_callbacks);

	// enable bluetooth
	bt_enable(NULL);

	settings_load();

	k_work_submit(&advertise_acceptlist_work);

	// // start advertising
	// bt_le_adv_start(adv_param, ad, ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd));

	printk("This is BLE demo! %s\n", CONFIG_BOARD);

	while(true) {
		if(indicate_enabled)
		{
			indicate_flag = !indicate_flag;
			indicate_send(indicate_flag);
		}
		if(notify_enabled)
		{
			notify_flag = !notify_flag;
			notify_send(notify_flag);
		}
		if(unpair_flag)
		{
			unpair_flag = false;
			unpair_device();
		}
		k_sleep(K_MSEC(5000));
	}

	return 0;
}
